#include "car_opr.h"
#include "rk3399_gpio_opr.h"
#include <linux/kernel.h>

#define ENA "D0"
#define DIR_IN_1 "C4"
#define DIR_IN_2 "C7"
#define ENB "D1"
#define DIR_IN_3 "C5"
#define DIR_IN_4 "C6"

static void left_wheels_accelerate(void);
static void right_wheels_accelerate(void);
static void set_dircection(int dir);
static void left_wheels_slide(void);
static void right_wheels_slide(void);

void car_init(void) {
	init_gpio();
	printk(KERN_INFO "car init \n");
	set_gpio2_dir(get_specific_gpio_pin(ENA), 1);
	set_gpio2_dir(get_specific_gpio_pin(DIR_IN_1), 1);
	set_gpio2_dir(get_specific_gpio_pin(DIR_IN_2), 1);
	set_gpio2_dir(get_specific_gpio_pin(ENB), 1);
	set_gpio2_dir(get_specific_gpio_pin(DIR_IN_3), 1);
	set_gpio2_dir(get_specific_gpio_pin(DIR_IN_4), 1);
}

void move_forward(void) {
	set_dircection(FORWARD);
	left_wheels_accelerate();
	right_wheels_accelerate();
}
void move_backward(void) {
	set_dircection(BACKWARD);
	left_wheels_accelerate();
	right_wheels_accelerate();
}
void turn_left(void) {
	left_wheels_slide();
}
void turn_right(void) {
	right_wheels_slide();
}
void brake(void) {
	set_dircection(STOP);
}
void slide(void) {
	left_wheels_slide();
	right_wheels_slide();
}
void restore_direction(void) {
	left_wheels_accelerate();
	right_wheels_accelerate();
}
static void left_wheels_accelerate(void) {
	set_gpio2_status(get_specific_gpio_pin(ENA), 1);
}
static void right_wheels_accelerate(void) {
	set_gpio2_status(get_specific_gpio_pin(ENB), 1);
}

static void set_dircection(int dir) {

	switch (dir) {
	case FORWARD:
		set_gpio2_status(get_specific_gpio_pin(DIR_IN_1), 0);
		set_gpio2_status(get_specific_gpio_pin(DIR_IN_2), 1);
		set_gpio2_status(get_specific_gpio_pin(DIR_IN_3), 0);
		set_gpio2_status(get_specific_gpio_pin(DIR_IN_4), 1);
		break;

	case BACKWARD:
		set_gpio2_status(get_specific_gpio_pin(DIR_IN_1), 1);
		set_gpio2_status(get_specific_gpio_pin(DIR_IN_2), 0);
		set_gpio2_status(get_specific_gpio_pin(DIR_IN_3), 1);
		set_gpio2_status(get_specific_gpio_pin(DIR_IN_4), 0);
		break;

	case STOP:
		set_gpio2_status(get_specific_gpio_pin(DIR_IN_1), 1);
		set_gpio2_status(get_specific_gpio_pin(DIR_IN_2), 1);
		set_gpio2_status(get_specific_gpio_pin(DIR_IN_3), 1);
		set_gpio2_status(get_specific_gpio_pin(DIR_IN_4), 1);
		break;
	default:
		break;
	}
}

static void left_wheels_slide(void) {
	set_gpio2_status(get_specific_gpio_pin(ENA), 0);
}

static void right_wheels_slide(void) {
	set_gpio2_status(get_specific_gpio_pin(ENB), 0);
}